#ifndef BTL_VIEWGEOMETRY_ESSENTIALMATRIX
#define BTL_VIEWGEOMETRY_ESSENTIALMATRIX

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace btl
{
namespace viewgeometry
{

template <typename MT>
Eigen::Matrix<typename MT::Scalar,3,3>
   cross_matrix(const Eigen::MatrixBase<MT>& v);

template <typename MT>
Eigen::Matrix<typename MT::Scalar,3,3>
   matrix3_cofactors(const Eigen::MatrixBase<MT>& m);

class BuildEssentialMatrix
{
   public:
      BuildEssentialMatrix();
      BuildEssentialMatrix(const Eigen::Isometry3d& Rt);
      void compute(const Eigen::Isometry3d& Rt);

      const Eigen::Matrix3d& matrix() const;

   private:
      Eigen::Matrix3d _matrix;
};

/// Decompose an Essential matrix into Rotation and Translation
/// using the SVD (according to Hartley & Zisserman, § 9.6.2)
class DecomposeEssentialSVD
{
   public:
      enum DecomposeOptions {
         ComputeNormalisedT = 1 << 0,
         ComputeNormalisedE = 1 << 1,
      };

      DecomposeEssentialSVD();
      DecomposeEssentialSVD(const Eigen::Matrix3d& E, int opts = 0);
      void compute(const Eigen::Matrix3d& E, int opts = 0);

      const Eigen::Matrix3d& rotationX() const;
      const Eigen::Matrix3d& rotationY() const;
      const Eigen::Vector3d& translation() const;

      Eigen::Isometry3d transform() const;
      Eigen::Isometry3d transformA() const;
      Eigen::Isometry3d transformB() const;
      Eigen::Isometry3d transformC() const;
      Eigen::Isometry3d transformD() const;

      const Eigen::Matrix3d& normalisedE() const;

   private:
      Eigen::Matrix3d _Rx;
      Eigen::Matrix3d _Ry;
      Eigen::Vector3d _t;
      Eigen::Matrix3d _Enorm;
};

/// Decompose an Essential matrix into Rotation and Translation
/// using cofactors (according to Horn, 1990, ``Recovering Baseline
/// and Orientation from `Essential' Matrix'')
class DecomposeEssentialHorn
{
   public:
      enum DecomposeOptions {
         ComputeNormalisedT = 1 << 0,
         ComputeNormalisedE = 1 << 1,
      };

      DecomposeEssentialHorn();
      DecomposeEssentialHorn(const Eigen::Matrix3d& E, int opts = 0);
      void compute(const Eigen::Matrix3d& E, int opts = 0);

      const Eigen::Matrix3d& rotationX() const;
      const Eigen::Matrix3d& rotationY() const;
      const Eigen::Vector3d& translation() const;

      Eigen::Isometry3d transform() const;
      Eigen::Isometry3d transformA() const;
      Eigen::Isometry3d transformB() const;
      Eigen::Isometry3d transformC() const;
      Eigen::Isometry3d transformD() const;

      const Eigen::Matrix3d& normalisedE() const;

   private:
      Eigen::Matrix3d _Rplus;
      Eigen::Matrix3d _Rminus;
      Eigen::Vector3d _t;
      Eigen::Matrix3d _Enorm;
};

} // namespace viewgeometry
} // namespace btl

namespace btl
{

using viewgeometry::BuildEssentialMatrix;
using viewgeometry::DecomposeEssentialHorn;
using viewgeometry::DecomposeEssentialSVD;

} // namespace btl

// ====================================================================
// === Implementation

namespace btl
{
namespace viewgeometry
{

template <typename MT>
inline Eigen::Matrix<typename MT::Scalar,3,3>
      cross_matrix(const Eigen::MatrixBase<MT>& v)
{
   assert(v.rows() == 3 && v.cols() == 1);
   return
      (Eigen::Matrix<typename MT::Scalar,3,3>() <<
          0.0, -v(2),  v(1),
          v(2), 0.0 , -v(0),
         -v(1), v(0),  0.0
      ).finished();
}

template <typename MT>
inline Eigen::Matrix<typename MT::Scalar,3,3>
      matrix3_cofactors(const Eigen::MatrixBase<MT>& m)
{
   assert(m.rows() == 3 && m.cols() == 3);
   return
      (Eigen::Matrix<typename MT::Scalar,3,3>() <<
         m.col(1).cross(m.col(2)).transpose(),
         m.col(2).cross(m.col(0)).transpose(),
         m.col(0).cross(m.col(1)).transpose()
      ).finished();
}

inline BuildEssentialMatrix::BuildEssentialMatrix():
   _matrix(Eigen::Matrix3d::Identity()) {}

inline BuildEssentialMatrix::BuildEssentialMatrix(const Eigen::Isometry3d& Rt)
{
   compute(Rt);
}

inline void BuildEssentialMatrix::compute(const Eigen::Isometry3d& Rt)
{
   _matrix = cross_matrix(Rt.translation()) * Rt.rotation();
}

inline const Eigen::Matrix3d& BuildEssentialMatrix::matrix() const
{
   return _matrix;
}

inline DecomposeEssentialSVD::DecomposeEssentialSVD() {}

inline DecomposeEssentialSVD::DecomposeEssentialSVD(const Eigen::Matrix3d& E, int opts)
{
   compute(E, opts);
}

inline const Eigen::Matrix3d& DecomposeEssentialSVD::rotationX() const
{
   return _Rx;
}

inline const Eigen::Matrix3d& DecomposeEssentialSVD::rotationY() const
{
   return _Ry;
}

inline const Eigen::Vector3d& DecomposeEssentialSVD::translation() const
{
   return _t;
}

inline Eigen::Isometry3d DecomposeEssentialSVD::transform() const
{
   return transformA();
}

inline Eigen::Isometry3d DecomposeEssentialSVD::transformA() const
{
   return Eigen::Translation3d(_t) * Eigen::Isometry3d(_Rx);
}

inline Eigen::Isometry3d DecomposeEssentialSVD::transformB() const
{
   return Eigen::Translation3d(-_t) * Eigen::Isometry3d(_Rx);
}

inline Eigen::Isometry3d DecomposeEssentialSVD::transformC() const
{
   return Eigen::Translation3d(_t) * Eigen::Isometry3d(_Ry);
}

inline Eigen::Isometry3d DecomposeEssentialSVD::transformD() const
{
   return Eigen::Translation3d(-_t) * Eigen::Isometry3d(_Ry);
}

inline const Eigen::Matrix3d& DecomposeEssentialSVD::normalisedE() const
{
   return _Enorm;
}

inline DecomposeEssentialHorn::DecomposeEssentialHorn() {}

inline DecomposeEssentialHorn::DecomposeEssentialHorn(const Eigen::Matrix3d& E, int opts)
{
   compute(E, opts);
}

inline const Eigen::Matrix3d& DecomposeEssentialHorn::rotationX() const
{
   return _Rplus;
}

inline const Eigen::Matrix3d& DecomposeEssentialHorn::rotationY() const
{
   return _Rminus;
}

inline const Eigen::Vector3d& DecomposeEssentialHorn::translation() const
{
   return _t;
}

inline Eigen::Isometry3d DecomposeEssentialHorn::transform() const
{
   return transformA();
}

inline Eigen::Isometry3d DecomposeEssentialHorn::transformA() const
{
   return Eigen::Translation3d(_t) * Eigen::Isometry3d(_Rplus);
}

inline Eigen::Isometry3d DecomposeEssentialHorn::transformB() const
{
   return Eigen::Translation3d(-_t) * Eigen::Isometry3d(_Rminus);
}

inline Eigen::Isometry3d DecomposeEssentialHorn::transformC() const
{
   return Eigen::Translation3d(_t) * Eigen::Isometry3d(_Rminus);
}

inline Eigen::Isometry3d DecomposeEssentialHorn::transformD() const
{
   return Eigen::Translation3d(-_t) * Eigen::Isometry3d(_Rplus);
}

inline const Eigen::Matrix3d& DecomposeEssentialHorn::normalisedE() const
{
   return _Enorm;
}

} // namespace viewgeometry
} // namespace btl

#endif // BTL_VIEWGEOMETRY_ESSENTIALMATRIX
